#include "cancomm.hpp"
#include <cstring>
#include <iostream>
#include "spdlog/spdlog.h"
#include <fcntl.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/sockios.h>
#include <unistd.h>
#include <netinet/in.h>
#include <fstream>
#include <cstdlib>

CanComm::CanComm(const std::string& interface,unsigned int bitrate)
    : interface_(interface),bitrate_(bitrate){}

CanComm::~CanComm() {
    shutdown();
}

bool CanComm::initialize() {
    std::string state_file = "/sys/class/net/" + interface_ + "/operstate";
    std::ifstream ifs(state_file);
    std::string state;
    if (ifs) {
        ifs >> state;
    }
    if (!ifs || state != "up") {
        spdlog::warn("Interface {} is down, attempting to bring it up...", interface_);
        std::string command = "sudo ip link set " + interface_ + " up type can bitrate " + std::to_string(bitrate_);
        int ret = system(command.c_str());
        if (ret != 0) {
            spdlog::error("Failed to bring up interface {} (command: {}), error: {}", interface_, command, ret);
            return false;
        }
    }

    socket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (socket_ < 0) {
        spdlog::error("Failed to create CAN socket for interface: {}", interface_);
        return false;
    }

    struct ifreq ifr;
    std::strncpy(ifr.ifr_name, interface_.c_str(), IFNAMSIZ);
    if (ioctl(socket_, SIOCGIFINDEX, &ifr) < 0) {
        spdlog::error("Failed to get index for CAN interface: {}", interface_);
        close(socket_);
        socket_ = -1;
        return false;
    }
    
    struct sockaddr_can addr;
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;
    
    if (bind(socket_, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        spdlog::error("Failed to bind CAN socket to interface: {}", interface_);
        close(socket_);
        socket_ = -1;
        return false;
    }
    
    int flags = fcntl(socket_, F_GETFL, 0);
    if (flags < 0 || fcntl(socket_, F_SETFL, flags | O_NONBLOCK) < 0) {
        spdlog::warn("Failed to set non-blocking mode for CAN socket");
    }
    
    running_ = true;
    readThread_ = std::thread(&CanComm::readThreadFunc, this);
    processThread_ = std::thread(&CanComm::processReceivedData, this);
    
    spdlog::info("CAN interface {} initialized successfully", interface_);
    return true;
}

bool CanComm::send(const std::vector<uint8_t>& data) {
    if (socket_ < 0 || !running_) return false;
    
    struct can_frame frame;
    frame.can_id = 0x123; 
    frame.can_dlc = data.size() > CAN_MAX_DLEN ? CAN_MAX_DLEN : data.size();
    
    std::memcpy(frame.data, data.data(), frame.can_dlc);
    
    ssize_t bytes_sent = write(socket_, &frame, sizeof(frame));
    return bytes_sent == sizeof(frame);
}

std::vector<uint8_t> CanComm::receive() {
    std::unique_lock<std::mutex> lock(dataMutex_);
    if (receiveQueue_.empty()) {
        return {};
    }
    
    auto data = receiveQueue_.front();
    receiveQueue_.pop();
    return data;
}

void CanComm::registerCallback(DataReceivedCallback callback) {
    callback_ = std::move(callback);
}

std::string CanComm::getName() const {
    return "CAN:" + interface_;
}

bool CanComm::isConnected() const {
    return socket_ >= 0 && running_;
}

void CanComm::shutdown() {
    running_ = false;
    
    if (readThread_.joinable()) {
        readThread_.join();     
    }
    
    if (processThread_.joinable()) {
        dataCond_.notify_all();
        processThread_.join();
    }
    
    if (socket_ >= 0) {
        close(socket_);
        socket_ = -1;
    }
}

void CanComm::readThreadFunc() {
    struct can_frame frame;
    
    while (running_) {
        ssize_t bytes_read = read(socket_, &frame, sizeof(frame));
        if (bytes_read == sizeof(frame)) {
            std::vector<uint8_t> data;
            
            data.push_back(static_cast<uint8_t>((frame.can_id >> 24) & 0xFF));
            data.push_back(static_cast<uint8_t>((frame.can_id >> 16) & 0xFF));
            data.push_back(static_cast<uint8_t>((frame.can_id >> 8) & 0xFF));
            data.push_back(static_cast<uint8_t>(frame.can_id & 0xFF));        
            
            data.insert(data.end(), frame.data, frame.data + frame.can_dlc);
            
            std::unique_lock<std::mutex> lock(dataMutex_);
            receiveQueue_.push(std::move(data));
            dataCond_.notify_one();
        } else if (bytes_read < 0) {
            if (errno != EAGAIN && errno != EWOULDBLOCK) {
                spdlog::error("CAN read error: {}", strerror(errno));
                running_ = false;
            }
        }
        
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
    }
}

void CanComm::processReceivedData() {
    while (running_) {
        std::unique_lock<std::mutex> lock(dataMutex_);
        dataCond_.wait(lock, [this] { return !running_ || !receiveQueue_.empty(); });
        
        if (!running_) break;
        
        if (!receiveQueue_.empty()) {
            auto data = receiveQueue_.front();
            receiveQueue_.pop();
            
            if (callback_) {
                lock.unlock();
                callback_(data);
                lock.lock();
            }
        }
    }
}